//应用程序模板 
#include <SetWaitForBump.h>
#include <SetMotorStraightTurn.h>
#include <SetCarTurn.h>
#include <SetWaitForAngle128.h>
#include <SetWaitforDarker.h>
#include <SetWaitforDark.h>
#include <SetCarTurn.h>
#include <SetMotorStraightTurn.h>
#include <SetWaitForStraight.h>
#include <GetChoiceBox.h>
#include <GetData.h>
#include <SetData.h>
#include <GetTouchScreenX.h>
#include <GetTouchScreenY.h>
#include <GetRightButton.h>
#include <GetLeftButton.h>
#include <GetTouch.h>
#include <GetLightSensor.h>
#include <SetWaitForStraight.h>
#include <SetMotorStraightAngle.h>
#include <SetMotorStraight.h>
#include <SetMotorServo48.h>
#include <SetMotorServo.h>
#include <SetAICamData.h>
#include "HardwareInfo.c"

#include <JMLib.c>

#include <SetWaitForTime.h>
#include <GetSysTime.h>

#include <GetTraceV2I2C.h>
#include <SetTraceV2BeepI2C.h>

#include <SetLCDClear.h>
#include <SetDisplayString.h>
#include <SetDisplayVar.h>
#include <SetDisplayVariable.h>

#include <SetMotor.h>
#include <SetMotorCode.h>
#include <GetMotorCode.h>

#include "HardwareInfo.c"
#include "JMLib.c"
#include <SetAICamData.h>
#include <SetWaitForAngle.h>
#include <GetAICamData.h>
#include <SetDisplayVar.h>
#include <SetInBeep.h>
#include <SetWaitForTime.h>


//老师写的

u16 G1,G2,G3,G4,G5;
u16 tempCS;
unsigned int S1,S2,S3,S4,S5,S6,S7;
int mode;
unsigned int g_temp;
double line_proportion;   //巡线抖动
long T15;
long light_ws;
unsigned int try;

int getBit(ch,bit){
	return (ch)&(0x01<<((bit)-1))?1:0;
}

void speed_control(int spl, int spr)
{
    if ( spl>100 )
    {
        spl=100;
    }
    if ( spl<-100 )
    {
        spl=-100;
    }
    if ( spr>100 )
    {
        spr=100;
    }
    if ( spr<-100 )
    {
        spr-100;
    }
    SetMotor(_M1_, spl);
    SetMotor(_M2_, spr);
}

void go_bmp(int sp, unsigned long bmp)
{
    long vl = 0;
    long vr = 0;
    SetMotorCode(_M1_);
    SetMotorCode(_M2_);
    bmp=abs(bmp);
    SetMotorServo(_M1_, sp, bmp);
    SetMotorServo(_M2_, sp, bmp);
    while (1)
    {
        vl = GetMotorCode(_M1_);
        vr = GetMotorCode(_M2_);
        if ( abs(vl)>=bmp||abs(vr)>=bmp )
        {
            break;
        }
    }
    SetInBeep(ON);
    SetWaitForTime(0.05);
    SetInBeep(OFF);
    speed_control(0, 0);
}


void light01()
{
    unsigned int PP1 = 0;
    unsigned int PP2 = 0;
    unsigned int PP3 = 0;
    unsigned int PP4 = 0;
    unsigned int PP5 = 0;
    unsigned char var0 = 0;
    if(!tempCS)
    {
    	//首次启动光电延时
    	S1 = GetLightSensor(_P1_);
        S2 = GetLightSensor(_P2_);
        S3 = GetLightSensor(_P3_);
        S4 = GetLightSensor(_P4_);
        S5 = GetLightSensor(_P6_);
        
        //首次启动写入光电判断阀值
        /*G1=GetData(1);
        G2=GetData(2);
        G3=GetData(3);
        G4=GetData(4);
        G5=GetData(5);*/
        SetWaitForTime(0.01);
        tempCS=1;
    }
    var0 = GetTraceV2I2C(_P1_, 9);
    S1 = getBit(var0, 1);
    S2 = getBit(var0, 2);
    S3 = getBit(var0, 3);
    S4 = getBit(var0, 4);
    S5 = getBit(var0, 5);
    S6 = getBit(var0, 6);
    S7 = getBit(var0, 7);
    	
    if(mode==1)
    {
    	S1 = !S1;
        S2 = !S2;
        S3 = !S3;
        S4 = !S4;
        S5 = !S5;
        S6 = !S6;
        S7 = !S7;
    } 
}

void goline(int sp)
{
    //extern global var
    extern unsigned int S1;
    extern unsigned int S2;
    extern unsigned int S3;
    extern unsigned int S4;
    //extern unsigned int S5;
    //extern unsigned int g_temp;
    //extern double line_proportion;   //巡线抖动
    //extern long T15;
    //extern long light_ws;
    //extern unsigned int S7;
    //extern unsigned int S6;

    int spl = 0;
    int spr = 0;
    long var0 = 0;
    long vt = 200;
    var0 = GetSysTime();
    //读取光电状态
    light01();
    if ( S1 )
    {
        g_temp=1;
        T15 = GetSysTime();
    }
    else
    {
        if ( S7 )
        {
            g_temp=7;
            T15 = GetSysTime();
        }
    }
    if ( S4 )
    {
        spl=sp;
        spr=sp;
        if ( var0-T15>=vt )
        {
            g_temp=4;
        }
    }
    else
    {
        if ( S3&&S4 )
        {
            spl=sp*0.8;
            spr=sp;
        }
        else
        {
            if ( S4&&S5 )
            {
                spl=sp;
                spr=sp*0.8;
            }
            else
            {
                if ( S3 )
                {
                    spl=sp*line_proportion;
                    spr=sp;
                    if ( var0-T15>=vt )
                    {
                        g_temp=3;
                    }
                }
                else
                {
                    if ( S5 )
                    {
                        spl=sp;
                        spr=sp*line_proportion;
                        if ( var0-T15>=vt )
                        {
                            g_temp=5;
                        }
                    }
                    else
                    {
                        if ( S2 )
                        {
                            spl=-0;
                            spr=sp;
                            if ( var0-T15>=vt )
                            {
                                g_temp=2;
                            }
                        }
                        else
                        {
                            if ( S6 )
                            {
                                spl=sp;
                                spr=-0;
                                if ( var0-T15>=vt )
                                {
                                    g_temp=6;
                                }
                            }
                            else
                            {
                                if ( S1 )
                                {
                                    spl=-10;
                                    spr=50;
                                }
                                else
                                {
                                    if ( S7 )
                                    {
                                        spl=50;
                                        spr=-10;
                                    }
                                    else
                                    {
                                        if ( g_temp==1 )
                                        {
                                            spl=-40;
                                            spr=50;
                                        }
                                        else
                                        {
                                            if ( g_temp==7 )
                                            {
                                                spl=50;
                                                spr=-40;
                                            }
                                            else
                                            {
                                                if ( g_temp==2 )
                                                {
                                                    spl=sp*0.8;
                                                    spr=sp;
                                                }
                                                else
                                                {
                                                    if ( g_temp==6 )
                                                    {
                                                        spl=sp;
                                                        spr=sp*0.8;
                                                    }
                                                    else
                                                    {
                                                        if ( g_temp==3 )
                                                        {
                                                            spl=sp;
                                                            spr=sp;
                                                        }
                                                        else
                                                        {
                                                            spl=sp;
                                                            spr=sp;
                                                        }
                                                    }
                                                }
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    }
    speed_control(spl, spr);
}

void catch_line(int sp, int light_w)
{
    // extern global var
    //extern unsigned int S1;
    //extern unsigned int S2;
    //extern unsigned int S3;
    //extern unsigned int S4;
    //extern unsigned int S5;
    //extern long light_ws;
    //extern unsigned int S7;
    //extern unsigned int S6;

    line_time(sp, 200);
    if ( light_w>=15 )
    {
        while (1)
        {
            goline(sp);
            if ( S1&&S7 )
            {
                break;
            }
        }
    }
    else
    {
        if ( light_w==1 )
        {
            while (1)
            {
                goline(sp);
                if ( S1 )
                {
                    break;
                }
            }
        }
        else
        {
            if ( light_w==7 )
            {
                while (1)
                {
                    goline(sp);
                    if ( S7 )
                    {
                        break;
                    }
                }
            }
            else
            {
                while (1)
                {
                    goline(sp);
                    if ( S1||S7 )
                    {
                        break;
                    }
                }
            }
        }
    }
    SetTraceV2BeepI2C(_P1_, 2);
    speed_control(0, 0);
}

void turn_w(int spl, int spr, int whi)
{
    // extern global var
    extern unsigned int S1;
    extern unsigned int S2;
    extern unsigned int S3;
    extern unsigned int S4;
    extern unsigned int S5;
    extern unsigned int S6;
    extern unsigned int S7;

    speed_control(spl, spr);
    SetWaitForTime(0.15);
    while (1)
    {
        light01();
        if ( whi==1 )
        {
            if ( S1 )
            {
                break;
            }
        }
        else
        {
            if ( whi==2 )
            {
                if ( S2 )
                {
                    break;
                }
            }
            else
            {
                if ( whi==3 )
                {
                    if ( S3 )
                    {
                        break;
                    }
                }
                else
                {
                    if ( whi==4 )
                    {
                        if ( S4 )
                        {
                            break;
                        }
                    }
                    else
                    {
                        if ( whi==5 )
                        {
                            if ( S5 )
                            {
                                break;
                            }
                        }
                        else
                        {
                            if ( whi==6 )
                            {
                                if ( S6 )
                                {
                                    break;
                                }
                            }
                            else
                            {
                                if ( S7 )
                                {
                                    break;
                                }
                            }
                        }
                    }
                }
            }
        }
    }
    speed_control(0, 0);
}

void rurnleft()
{
    turn_w(-30, 50, 4);
}

void turnright()
{
    turn_w(50, -50, 4);
}

//xxjuli
void line_bmp(int sp, unsigned long bmp, int num)
{   
    // extern global var
    //extern unsigned int try;

    long vbmp = 0;
    long vbmp1 = 0;
    long vl = 0;
    long vr = 0;
    long D = 0;
    long spl = 0;
    long spr = 0;
    long cp = 0;
    long R = 0;
    long cm = 0;
    long L = 0;
    if ( num==0 )
    {
        SetMotorCode(_M1_);
        SetMotorCode(_M2_);
        bmp=fabs(bmp);
        while (1)
        {
            goline(sp);
            vbmp = GetMotorCode(_M1_);
            vbmp1 = GetMotorCode(_M2_);
            if ( fabs(vbmp)>=bmp||fabs(vbmp1)>=bmp )
            {
                speed_control(0, 0);
                break;
            }
        }
    }
    if ( num!=0&&try==0 )
    {
        bmp = GetData(num);
        SetMotorCode(_M1_);
        SetMotorCode(_M2_);
        bmp=abs(bmp);
        speed_control(sp, sp);
        while (1)
        {
            vl = GetMotorCode(_M1_);
            vr = GetMotorCode(_M2_);
            goline(sp);
            if ( abs(vl)>=bmp||abs(vr)>=bmp )
            {
                break;
            }
        }
        SetInBeep(ON);
        SetWaitForTime(0.05);
        SetInBeep(OFF);
        speed_control(0, 0);
    }
    if ( num!=0&&try==1 )
    {
        speed_control(0, 0);
        SetMotorCode(_M1_);
        SetMotorCode(_M2_);
        while (1)
        {
            R = GetRightButton();
            if ( R==1 )
            {
                bmp = GetData(num);
                SetMotorCode(_M1_);
                SetMotorCode(_M2_);
                bmp=abs(bmp);
                speed_control(sp, sp);
                while (1)
                {
                    vl = GetMotorCode(_M1_);
                    vr = GetMotorCode(_M2_);
                    goline(sp);
                    if ( abs(vl)>=bmp||abs(vr)>=bmp )
                    {
                        break;
                    }
                }
                SetInBeep(ON);
                SetWaitForTime(0.05);
                SetInBeep(OFF);
                speed_control(0, 0);
                break;
            }
            cp = GetTouch(_P6_);
            while ( cp==1 )
            {
                cp = GetTouch(_P6_);
                vl = GetMotorCode(_M1_);
                vr = GetMotorCode(_M2_);
                goline(sp);
            }
            speed_control(0, 0);
            SetDisplayVar(1, vl, YELLOW, BLACK);
            SetDisplayVar(2, vr, YELLOW, BLACK);
            L = GetLeftButton();
            if ( L==1 )
            {
                if ( abs(vl)>abs(vr) )
                {
                    vl=abs(vl);
                    SetData(num, vl);
                }
                else
                {
                    vr=abs(vr);
                    SetData(num, vr);
                }
                SetWaitForTime(1);
                break;
            }
            cm = GetTouchScreenX();
            GetTouchScreenY();
            if ( cm>=120 )
            {
                vr=vr+10;
                vl=vl+10;
            }
            else
            {
                if ( cm<120&&cm>0 )
                {
                    vr=vr-10;
                    vl=vl-10;
                }
            }
            while ( GetTouchScreenX()>0||GetTouchScreenY()>0 )
            {
		;
            }
        }
        SetInBeep(ON);
        SetWaitForTime(0.05);
        SetInBeep(OFF);
        speed_control(0, 0);
    }
    SetLCDClear(BLACK);
}

void gotime(int sp, double time)
{
    speed_control(sp, sp);
    SetWaitForTime(time);
    speed_control(0, 0);
}


//lao shi jie shu 


#define COLOR_BLACK 1
#define COLOR_WHITE 0

//  黑路白边
//#define ROAD_COLOR	COLOR_BLACK
//#define SIDE_COLOR	COLOR_WHITE
//  白路黑边
#define ROAD_COLOR	COLOR_WHITE
#define SIDE_COLOR	COLOR_BLACK

// 获得光电值。ch为光电返回的数据，bit为第几个光电
#define get_bit(ch,bit)	(ch)&(0x01<<((bit)-1))?ROAD_COLOR:SIDE_COLOR   

// 当前运行状态定义
#define MID	0
#define LEFT	-1
#define RIGHT	1
#define MID_LEFT	-2
#define MID_RIGHT	2
#define CROSS 3
#define UNKNOWN 4

// 端口定义
#define MOTOR_LEFT	_M1_
#define MOTOR_RIGHT	_M2_
#define MOTOR_ARM	_M4_
#define SENSOR_LIGHT	_P1_

// 速度定义，系统使用SPEED，调试时可以用高中低速
#define SPEED_HIGH	70
#define SPEED_MID	50
#define SPEED_LOW	30
#define SPEED	SPEED_HIGH

#define fl	car_go_at_cross();car_turn_catch_line(LEFT)
#define fr	car_go_at_cross();car_turn_catch_line(RIGHT)

// trace light values:
unsigned int L, L2, L1, M, R1, R2, R;

// PID
float pid_p=0.9;
float pid_i=0.3;
float pid_d=0.0;
float pid;


// 函数命名规则：
// trace_、motor_、arm_直接获取或控制模块
// car_、tank_控制车辆本身
// action_做任务

void l(){
//catchline(SPEED,1);
//rurnleft();
}

void r(){
//catchline(SPEED,7);
//rurnright();
}

// 通过光电获得当前运行状态，返回值如下
// MID：走在路的正中……不用处理，直接走
// LEFT：到了左转路口……可以左转了
// RIGHT：到了右转路口……可以右转了
// CROSS：到了十字路口……我也不知道该往那边转
// MID_LEFT：太偏左了……需要找线回中
// MID_RIGHT：太偏右了……需要找线回中
// UNKNOWN：我也不知道是什么情况，看起来传感器不正确
// 其他值：与主路偏转的程度，正数偏右，负数偏左，用于PID算法
int trace_get_status(){
	unsigned char d;

	// 64 32 16  8  4   2   1
	// S7 S6 S5 S4 S3 S2 S1/
	// R   R2  R1 M  L1  L2   L
	d=GetTraceV2I2C(SENSOR_LIGHT, 9);
	R=get_bit(d,7);
	R2=get_bit(d,6);
	R1=get_bit(d,5);
	M=get_bit(d,4);
	L1=get_bit(d,3);
	L2=get_bit(d,2);
	L=get_bit(d,1);
//	unsigned long disp=80000000+L*1000000+L2*100000+L1*10000+M*1000+R1*100+R2*10+R;
	if(L&&R) return CROSS;
	if(L)	return LEFT;
	if(R)	return RIGHT;
	if(L2) return MID_LEFT;
	if(R2) return MID_RIGHT;

	if(d==8)	return MID;

	if(d==12)	return -10;
	if(d==4)	return -15;
	//if(d==6)	return -20;
	//if(d==2)	return -30;
	
	if(d==24)	return 10;
	if(d==16)	return 15;
	//if(d==48)	return 20;
	//if(d==32)	return 30;

	if(M)	return MID;

	return UNKNOWN;
}

// 设置左右轮的速度，由于这辆车是反着装的，需要调整控制器符号
void motor_set_speed(int speedL, int speedR)
{
	SetMotor(MOTOR_LEFT, speedL);
	SetMotor(MOTOR_RIGHT, speedR);
}

// 重置左右轮码盘值为0
void motor_set_code()
{
	SetMotorCode(MOTOR_LEFT);
	SetMotorCode(MOTOR_RIGHT);
}

// 获得码盘值，由于这辆车是反着装的，所以值加上了负号
int motor_get_code(int motor)
{
	return -GetMotorCode(motor);
}

// 让左右轮同时转动任意角度，到角度后不停车
// 可以用来做任务
void motor_spin_angle(long angleL, long angleR)
{
	if( (angleL==0)&&(angleR==0) ){
		return;
	}
	long total;
	long l;	
	long r;
	
	total = abs(angleL)+abs(angleR);
	motor_set_code();
	motor_set_speed(SPEED*angleL/total, SPEED*angleR/total);
	while( (abs(l)<=abs(angleL))&&(abs(r)<=abs(angleR)) ){
		l = GetMotorCode(MOTOR_LEFT);
		r = GetMotorCode(MOTOR_RIGHT);
	}
	//需要停车就在这里停一下：
	//motor_set_speed(0,0);
}

// 手臂旋转角度
void arm_servo_angle(int angle)
{
	SetMotorServo(MOTOR_ARM, SPEED_LOW, angle);
}

// 巡线中，当太偏左或者太偏右的时候，找回路中线
// 如果发现到了路口，则直接返回路口值MID、LEFT或者RIGHT
// 不停车
int  car_catch_line(int dir)
{
	int status;
	
	if(dir>0){ //向右转就让右边的轮子慢一些，反之亦然
		dir=1;
		motor_set_speed(SPEED, -SPEED*0.8);
	}else{
		dir=-1;
		motor_set_speed(-SPEED*0.8, SPEED);
	}

	status=trace_get_status();
	while( status!=MID && abs(status)<5 ){// 没有回正或者L、R、L2、R2四个光电依然扫在路上，则继续转
		if(status==LEFT || status==RIGHT || status==CROSS){ //如果发现其实是路口（即钝角转弯口），则停止回线，将探测到的转弯方向返回，准备进行转弯
			//motor_set_speed(0,0);
			return status;
		}
		status=trace_get_status();
	}
	return status;
}

// 检查巡线，不停车
int car_go_line()
{
	int status;
	float pid_old;

	status=trace_get_status();
	if(status==LEFT || status==RIGHT || status==CROSS){
		//motor_set_speed(0,0);
		return status;
	}else if( status==MID_LEFT || status==MID_RIGHT){
		car_catch_line(status);
	}else{
		if(status!=UNKNOWN)
		{
			pid_old=pid;
			pid*=pid_i;
			pid+=status;
			pid*=pid_p;
			if(pid>0){
				motor_set_speed(SPEED, SPEED*(100-pid)/100);
			}else if(pid<0){
				motor_set_speed(SPEED*(100+pid)/100, SPEED);
			}else{
				motor_set_speed(SPEED, SPEED);
			}
		}
	}
	return status;
}

// 巡线运行到路口，返回路口值，不停车
int car_go_at_cross()
{
	int status;
	float pid_old;
	while(1){
		status=trace_get_status();
 		if(status==LEFT || status==RIGHT || status==CROSS){
			//motor_set_speed(0,0);
			return status;
		}else if( status==MID_LEFT || status==MID_RIGHT){
			car_catch_line(status);
		}else{
			if(status!=UNKNOWN)
			{
				pid_old=pid;
				pid*=pid_i;
				pid+=status;
				pid*=pid_p;
				if(pid>0){
					motor_set_speed(SPEED, SPEED*(100-pid)/100);
				}else if(pid<0){
					motor_set_speed(SPEED*(100+pid)/100, SPEED);
				}else{
					motor_set_speed(SPEED, SPEED);
				}
			}
		}
	}
}

// 巡线运行一段距离或遇到路口，停车
int car_go_at_distance(long mm)
{
	int status;
	long left, right, dist;
	
	motor_set_code();
	dist = fabs(1.8*mm); // 将车行距离转为码盘值
        while (1){
		status=car_go_line();
		if(status==LEFT || status==RIGHT || status==CROSS){// 如果到路口了，就停止
			motor_set_speed(0, 0);
			return status;
		}else{
			left = fabs(motor_get_code(MOTOR_LEFT));
			right = fabs(motor_get_code(MOTOR_RIGHT));
			if ( left >= dist || right >= dist ){ // 如果码盘转够了，就停止
				motor_set_speed(0, 0);
				return status;
			}
		}
	}
}

// 到路口后根据方向转弯，不停车
void car_turn_catch_line(int dir)
{
	int status;

	// 过路口后向前前进50mm
	car_straight_distance(50);
	motor_set_speed(SPEED*dir, -SPEED*dir);
	
	status=trace_get_status();
	while( status!=MID && abs(status)<5 ){
		status=trace_get_status();
	}
	//motor_set_speed(0,0);
}

// 向前前进一段距离后停车，等待结束，系统函数，缓慢启动、缓慢停止
// 输入为前进距离mm数
// 可以用来做任务
void car_straight_wait_distance(long mm)
{
	SetWaitForStraight( MOTOR_LEFT, MOTOR_RIGHT, -SPEED, -1.8*mm);
}

// 向前前进一段距离后停车，不等待结束即执行后续命令，系统函数，缓慢启动、缓慢停止
// 输入为前进距离mm数
// 可以用来做任务
void car_straight_slow_distance(long mm)
{
	SetMotorStraightAngle(MOTOR_LEFT, MOTOR_RIGHT, -SPEED, -1.8*mm);
}

// 以当前SPEEd速度向前前进一段距离，不停车
// 输入为前进距离mm数
void car_straight_distance(long mm)
{
	motor_set_speed(SPEED, SPEED);
	motor_spin_angle(1.8*mm, 1.8*mm);
	motor_set_speed(0, 0);
}

// 以当前SPEEd速度向前前进一段时间，不停车
// 输入为前进持续时间s数
void car_straight_time(double time)
{
	motor_set_speed(SPEED, SPEED);
	SetWaitForTime(time);
	motor_set_speed(0, 0);
}

// 整车旋转，角度为车头偏转角
// 可以用来做任务
void car_turn_by_angle(int angle)
{
	int sp=-SPEED;
	if(angle<0){
		sp=SPEED;
		angle=-angle;
	}
	SetMotorStraightTurn(MOTOR_LEFT, MOTOR_RIGHT, sp, angle);
}

// 车原地旋转角度，停车
// 输入是整车旋转的角度，大概齐
// 可以用来做任务
void tank_turn(int angle)
{
	SetCarTurn(MOTOR_LEFT, MOTOR_RIGHT, -SPEED, 3.1*angle);
}

// 通过AI摄像头检查人，然后旋转90度的倍数。
void action_find_people()
{
	long count;
	
	SetAICamData(1, 0);
	arm_servo_angle(200);
	count = GetAICamData(1);
	//第一个人到底是否需要转360度？
	SetWaitForBump(_P3_);
	SetWaitForAngle(MOTOR_ARM, 100, 360*count); //不要这样做，action函数中应该去调用arm函数，形成层级关系，而不是越级指挥底层的M4
}

void test_go()
{
	int status;
	float pid_old;
	while(1){
		status=trace_get_status();
//		SetLCDClear(BLACK);
//		SetDisplayVar(1, status, YELLOW, BLACK);
 		if(status==LEFT || status==RIGHT){
//			motor_set_speed(0,0);
//			SetTraceV2BeepI2C(SENSOR_LIGHT, 2);
//			SetWaitForTime(2);
//			SetDisplayVar(6, status, WHITE, BLACK);
//			SetTraceV2BeepI2C(SENSOR_LIGHT, 1);
			//motor_spin_angle(50,50);
//			SetTraceV2BeepI2C(SENSOR_LIGHT, 2);
//			SetWaitForTime(2);
			car_turn_catch_line(status);
//			SetTraceV2BeepI2C(SENSOR_LIGHT, 4);
//			SetWaitForTime(2);
		}else if( status==MID_LEFT || status==MID_RIGHT){
			car_catch_line(status);
		}else if( status==CROSS){
//			SetTraceV2BeepI2C(SENSOR_LIGHT, 1);
			car_turn_catch_line(LEFT);
		}else{
//			SetTraceV2BeepI2C(SENSOR_LIGHT, 1);
			if(status!=UNKNOWN)
			{
				pid_old=pid;
				pid*=pid_i;
				pid+=status;
				pid*=pid_p;
				//pid=SPEED*(100-pid_i)*pid_p;
//				SetDisplayVar(1, pid, YELLOW, BLACK);
				if(pid>0){
					motor_set_speed(SPEED, SPEED*(100-pid)/100);
				}else if(pid<0){
					motor_set_speed(SPEED*(100+pid)/100, SPEED);
				}else{
					motor_set_speed(SPEED, SPEED);
				}
			}
		}
//		SetWaitForTime(0.01);
	}
}

int main(void)
{
	//E6RCU_Init();
//	car_straight_to_distance(2000);

	while(1){
		int test=GetChoiceBox("A:TEST   B:GEME");
		if (test==1){
			while(1){
				action_find_people();
				int i=  GetChoiceBox("A:break   B:continue");
				if(i==1){break;}
				if(i==2){;}
			}
		}
		if(test==2){
			go_bmp(60,300);//出基地
			line_bmp(60, 350, 0);
			tank_turn(90);
			go_bmp(60,300);
			gotime(60,200);
		}
	}
}

// 以下是测试用的

void test_turn_on_corss()
{
	int i,j;
	
	while(1){
		i=car_go_at_cross();
		motor_set_speed(0,0);
		j=GetChoiceBox("Next action: A=left, B=right, C=back, D=by status");
		switch(j){
		case 1:
			car_turn_catch_line(LEFT);
			break;
		case 2:
			car_turn_catch_line(RIGHT);
			break;
		case 3:
			if(i==LEFT){
				car_turn_catch_line(RIGHT);
			}
			if(i==RIGHT){
				car_turn_catch_line(LEFT);
			}
			if(i==CROSS){
				car_turn_catch_line(RIGHT);
				car_turn_catch_line(RIGHT);
			}
			break;
		case 4:
			if(i==LEFT){
				car_turn_catch_line(LEFT);
			}
			if(i==RIGHT){
				car_turn_catch_line(RIGHT);
			}
			if(i==CROSS){
				car_turn_catch_line(LEFT);
			}
			break;
		}
		motor_set_speed(0,0);
		GetChoiceBox("Press any key to cont...");
	}
}